/******************************************************************************
**
** File name:	rtos_debug_ex.S
**
** Description:	RTOS v2. Implementation of the RTOS debug_print service.
**				This service is intended to debug either application
**				and/or RTOS itself.
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**		void			RTOS_DebugOutArgs	( const char * fmt_str, ... )
**
**		void			RTOS_DebugOutSync   ( void * )
**		void			RTOS_DebugOutAddr   ( void * )
**		void			RTOS_DebugOutData	( void * , RTOS_U8_t )
**		void			RTOS_DebugOutDump	( void )
**
**		void			RTOS_HaltOS			( void )
**
** <<EXP>> RTOS Serial Debug Protocol v3 (extended):
**
**
**
**
**		TBD
**
**
**
**
** <<NEW>> RTOS Serial Debug Protocol v2 (optimized):
**
**		void			RTOS_DebugOutSync   ( void * )
**		void			RTOS_DebugOutAddr   ( void * )
**		void			RTOS_DebugOutData	( void * , RTOS_U8_t )
**		void			RTOS_DebugOutDump	( void )
**
**  1. Sending pointer to the constant string in ROM:
**
**		bits		7654 3210	comments
**		=======================================================================
**		byte_0		0110 cscs	bits 7..4 = ID; bits 3..0 = CS ((byte_1 XOR byte_2) & 0x0F)
**		byte_1		dddd dddd	address low byte  
**		byte_2		dddd dddd	address high byte 
**
**	2. Sending data buff
**
**		bits		7654 3210	comments
**		=======================================================================
**		byte_0		1001 nnnn	bits 7..4 = ID; bit 3..0 - (data_size-1)
**		byte_1		dddd dddd	data byte 1
**		...			...	
**		byte_N		dddd dddd	data byte N (N = data_size)
**		byte_N+1	cscs cscs	check sum ( 0 XOR byte_1 XOR byte_2 ... XOR byte_N )
**								particular case: for 1 byte (N = 1) check sum == byte
**								1001 0000  dddd dddd dddd dddd  
**
** Notes:	This module features are for debug purpose only! Not for use
**			in release versions.
**
*******************************************************************************
**
** History:
**
**	10/10/2011 - AK - Moving to RTOS version 2. Added services to access
**					RtosKernelData structure for debug purposes.
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2.
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
**	08/13/2012 - AK - RTOS_DebugOutXXX() services prototype.
**	08/24/2012 - AK - Optimized RTOS debug protocol
**	09/06/2012 - AK - Prototype of RTOS_DebugPrint
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_debug_ex)

        _CODESECTION

;******************************************************************************
;* Prototype:	void RTOS_DebugOutArgs( const char * fmt_str, ... )
;* Description: This is basic function for RTOS_DebugPrint() macro
;* Passed:		
;* Returns:		none
;* Uses:		none
;* Calls:		none
;* Assumes:		
;* Notes:		For Debug purpose only! Don't use in release version!
;******************************************************************************
	_FUNCTION(RTOS_DebugOutArgs)
	
RTOS_DebugOutArgs:

	PUSH	R28
	PUSH	R29

	IN		R28,_SFR_IO_ADDR(SPL)
	IN		R29,_SFR_IO_ADDR(SPH)

	LDD		R30,Y+4
	LDD		R31,Y+3
	
	;; The following code is to determine the number of parameters
	;; Don't try to understand how it works. It just works!!!
	
	LSL		R30
	ROL 	R31

	ADIW	R30,7
	
	LPM		R16,Z
	CPI		R16,0x90
	BRNE	two

four:

	LDI		R16,4
	RJMP	done

two:	

	SBIW	R30,4
	
	LPM		R16,Z
	CPI		R16,0x90
	BRNE	six

	LDI		R16,2
	RJMP	done
	
six:

	ADIW	R30,1

	LPM		R16,Z	;; this is correct for 2 < N <= 8, 		
					;; TBD analyse ADIW R24,0xXX opcode

done:				;; R16 = number (size on the stack) of parameters
					;; including fmt_str, so the min value is 2.
					
	ADIW	R28,5	;; Y points to first parameter (located on caller stack)
	
	LD		p1l,Y+
	LD		p1h,Y+
	
	rcall	RTOS_DebugOutAddr
	
	MOV		p1l,Yl
	MOV		p1h,Yh	;; p1l:p1h <- pointer to the caller stack that contains parameters
	
	CLR		p2h
	MOV		p2l,R16
	SUBI	p2l,2	;; p2l <- number of parameters in bytes 
	TST		p2l
	BREQ	skip
	
	DEC		p2l
	rcall	RTOS_DebugOutData
	
skip:
	
	POP		R29
	POP		R28
	
	ret

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutArgs)	

/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutPtr( void * data )
** Description:	This function put the passed parameter into the debug USART
**				queue. The data will be actually sent from RTOS idle task.
** Passed:		p1l:p1h - pointer to the constant debug string.
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		Debug sting is the constant located in program memory.
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutPtr)
	
RTOS_DebugOutPtr:

	PUSH	R0
	IN		R0,SREG
	CLI

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

dop_01:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_01

	STD		Z + 0, p1l		;; Send first data byte

dop_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_02

	STD		Z + 0, p1h		;; Send second data byte

	EOR		p1l,p1h

dop_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_03

	STD		Z + 0, p1l		;; Send checksum = xor ( p1l, p1h )

	OUT		SREG,R0
	POP		R0

ret

	;; TBD

	ret
	
	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutPtr)

/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutByte( RTOS_U8_t data )
** Description:	
** Passed:		p1l - data byte to log
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutByte)

RTOS_DebugOutByte:

	PUSH	R0
	IN		R0,SREG
	CLI

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

wt_01:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_01

	STD		Z + 0, p1l		;; Send first data byte

	LDI		p1h, 0x80		;; p1h <- log data identifier, message length = 0 (single data byte);

wt_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_02

	STD		Z + 0, p1h		;; Send second data byte

	EOR		p1l,p1h

wt_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_03

	STD		Z + 0, p1l		;; Send checksum = xor ( p1l, p1h )

	OUT		SREG,R0
	POP		R0

	ret
	
	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutByte)

/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutSync( void )
** Description:	send predefined byte sequence ( 0x33, 0x55, 0xAA )
** Passed:		none
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutSync)

RTOS_DebugOutSync:

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	PUSH	R0
	IN		R0,SREG
	CLI						;; Enter Critical Section

dos_wt_01:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_01		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0x33
	STD		Z + 0, p2l		;; Send 0x33

dos_wt_02:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_02		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0xAA
	STD		Z + 0, p2l		;; Send 0xAA

dos_wt_03:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_03		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0x55
	STD		Z + 0, p2l		;; Send 0x55


	OUT		SREG,R0			;; Leave Critical Section
	POP		R0			

	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutSync)


/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutAddr( void * addr )
** Description:	
** Passed:		p1l:p1h - pointer to the constant string in ROM
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutAddr)

RTOS_DebugOutAddr:

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	MOVW	p2l, p1l		;; Calculate ID/CS byte value
	EOR		p2l, p2h		;; p2l = LOW(addr) XOR HIGH(addr)
	ANDI	p2l, 0x0F		;; p2l &= 0x7F
	ORI		p2l, 0x60		;; p2l |= 0x60	

	PUSH	R0
	IN		R0,SREG
	CLI						;; Enter Critical Section

doa_wt_01:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	doa_wt_01		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2l		;; Send ID/CS byte

doa_wt_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	doa_wt_02		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1l		;; Send LOW(addr)

doa_wt_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	doa_wt_03		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1h		;; Send HIGH(byte)

	OUT		SREG,R0			;; Leave Critical Section
	POP		R0			

	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutAddr)

/******************************************************************************
** Prototype:	void RTOS_DebugOutDump( void )
** Description:	This function dumps the RTOS state
** Passed:		none
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/

	_FUNCTION( RTOS_DebugOutDump )

RTOS_DebugOutDump:

	ldi			p1l,lo8(RtosKernelData)
	ldi			p1h,hi8(RtosKernelData)

	ldi			p2l,RtosKernelDataSz-1

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutDump)

/******************************************************************************
** Prototype:	void RTOS_DebugOutData( void * data, RTOS_U8_t size )
** Description:	
** Passed:		p1l:p1h - pointer to data buff to log it out
**				p2l		- number of bytes in data buff 
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		size = 0..31 (max buff length is 32 bytes)
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutData)

RTOS_DebugOutData:

	PUSH	Yh				;; preserve Yl:Yh (frame pointer) 
	PUSH	Yl				;; from changes

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	MOVW	Yl, p1l			;; Yl:Yh <- p1l:p1h	(pointer to the data buff)
							;; R24, R25 (p1l:p1h) are free to reuse

	ANDI	p2l, 0x0F		;; Calculate ID/NN byte value
	ORI		p2l, 0x90		;; 

	PUSH	R0				;; Enter critical
	IN		R0,SREG
	CLI						

dod_wt_01:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_01		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2l		;; Send <<ID/NN>> byte

	ANDI	p2l, 0x0F		;; init counter
	INC		p2l
	CLR		p1h				;; reset check sum

dod_loop:

	LD		p2h, Y+			;; load data byte to p2h, increment pointer 
							;; p2h <- (Yl:Yh), Yl:Yh += 1

dod_wt_02:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_02		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2h		;; Send <<DATA>> byte

	EOR		p1h, p2h		;; update check sum 

	DEC		p2l				;; decrement byte counter
	BRNE	dod_loop		;; loop if not zero

dod_wt_03:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_03		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1h		;; Send <<CS>> byte

	OUT		SREG,R0			;; Leave critical
	POP		R0			


	POP		Yl				;; restore frame pointer 
	POP		Yh				;; (Yl:Yh) from stack


	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutData)

/******************************************************************************
** Prototype:	void RTOS_HaltOS(void)
** Description:	Lock the system. 
** Passed:		none
** Returns:		never returns
** Uses:		nothing
** Calls:		none
** Assumes:		Debug mode
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
    _FUNCTION(RTOS_HaltOS)

RTOS_HaltOS:

	break

    cli						;; BeginCritical			

rt_ht00:

    rjmp    rt_ht00			;; infinite loop

    _ENDFUNC	;; _FUNCTION(RTOS_Halt)

	_END
	
;;*****************************************************************************
;; Prototype:	void RTOS_KernelDump(void)
;; Description:	
;; Passed:		none
;; Returns:		
;; Uses:		
;; Calls:		none
;; Assumes:		Debug mode
;; Notes:		For Debug purpose only! Don't use in release version!
;;*****************************************************************************
    _FUNCTION(RTOS_KernelDump)

RTOS_KernelDump:	

	;; TBD
	
	RET
	
    _ENDFUNC	;; _FUNCTION(RTOS_KernelDump)

	_END
